/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of SeMoLearning.
 *
 * SeMoLearning is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SeMoLearning is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SeMoLearning.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "semol/core/Node.h"
#include "semol/core/Memory.h"
#include <opencv2/highgui/highgui.hpp>

#include <utilite/UStl.h>
#include <utilite/ULogger.h>

namespace semol
{

bool NeighborLink::updateSensorHistoryIds(int idFrom, int idTo)
{
	bool modified = false;
	for(std::list<std::vector<int> >::iterator iter=_sensorHistoryIds.begin(); iter!=_sensorHistoryIds.end(); ++iter)
	{
		for(std::vector<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
		if(*jter == idFrom)
		{
			*jter = idTo;
			modified = true;
		}
	}
	return modified;
}

bool NeighborLink::isSensorHistorySameIds() const
{
	if(_sensorHistoryIds.size())
	{
		const std::vector<int> & firstIds = *_sensorHistoryIds.begin();
		for(std::list<std::vector<int> >::const_iterator iter = _sensorHistoryIds.begin();
			iter != _sensorHistoryIds.end();
			++iter)
		{
			std::vector<int>::const_iterator fter = firstIds.begin();
			for(std::vector<int>::const_iterator jter = iter->begin();
				jter!=iter->end() && fter!=firstIds.end();
				++jter, ++fter)
			{
				if(*fter != *jter)
				{
					return false;
				}
			}
		}
		return true;
	}
	return false;
}

Node::~Node()
{
	UDEBUG("id=%d", _id);
}

Node::Node(int id, const std::vector<int> & sensors) :
		_id(id),
		_weight(0),
		_saved(false),
		_modified(true),
		_neighborsModified(true),
		_sensors(sensors)
{
}

void Node::addNeighbors(const NeighborsMultiMap & neighbors)
{
	for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
	{
		this->addNeighbor(i->second);
	}
}

void Node::addNeighbor(const NeighborLink & neighbor)
{
	UDEBUG("Add neighbor %d to %d with aid=%d (actuators=%d)",
			neighbor.toId(),
			this->id(),
			neighbor.actuatorId(),
			neighbor.actuators().size());
	_neighbors.insert(std::pair<int, NeighborLink>(neighbor.toId(), neighbor));
	if(neighbor.actuatorId())
	{
		_neighborsWithActuators.insert(neighbor.toId());
		_aids.insert(neighbor.actuatorId());
	}
	_neighborsAll.insert(neighbor.toId());
	_neighborsModified = true;
}

void Node::removeNeighbor(int neighborId, int actuatorId)
{
	int count = 0;
	for(NeighborsMultiMap::iterator iter = _neighbors.lower_bound(neighborId); iter!=_neighbors.end() && iter->first==neighborId; ++iter)
	{
		++count;
		if(iter->second.actuatorId() == actuatorId || actuatorId == -1)
		{
			UDEBUG("removing neighbor %d with aid=%d from signature %d", neighborId, iter->second.actuatorId(), this->id());
			_aids.erase(iter->second.actuatorId());
			_neighbors.erase(iter++);
			_neighborsModified = true;
			if(count == 1 && (iter == _neighbors.end() || iter->first != neighborId))
			{
				_neighborsWithActuators.erase(neighborId);
				_neighborsAll.erase(neighborId);
			}
			break;
		}
	}
}

void Node::removeNeighbors()
{
	if(_neighbors.size())
		_neighborsModified = true;
	_neighbors.clear();
	_neighborsWithActuators.clear();
	_neighborsAll.clear();
	_aids.clear();
}

void Node::changeNeighborIds(int idFrom, int idTo)
{
	std::pair<NeighborsMultiMap::iterator, NeighborsMultiMap::iterator> pair = _neighbors.equal_range(idFrom);
	if(pair.first != _neighbors.end() && pair.first != pair.second)
	{
		std::list<NeighborLink> linksToAdd;
		for(NeighborsMultiMap::iterator iter = pair.first; iter!=pair.second; ++iter)
		{
			NeighborLink link = iter->second;
			link.setId(idTo);
			linksToAdd.push_back(link);
		}
		_neighbors.erase(idFrom);
		_neighborsWithActuators.erase(idFrom);
		_neighborsAll.erase(idFrom);
		for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
		{
			_neighbors.insert(std::make_pair(iter->toId(), *iter));
			if(iter->actuators().size())
			{
				_neighborsWithActuators.insert(iter->toId());
			}
			_neighborsAll.insert(iter->toId());
		}
		_neighborsModified = true;
	}
	UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}

void Node::keepUniqueLink(int id)
{
	std::map<int, NeighborLink> links; // sort by aid
	for(NeighborsMultiMap::iterator iter = _neighbors.lower_bound(id); iter!=_neighbors.end() && iter->first == id;++iter)
	{
		links.insert(std::make_pair(iter->second.actuatorId(), iter->second));
	}
	if(links.size() > 1)
	{
		_neighbors.erase(id);
		_neighbors.insert(std::make_pair(id, links.rbegin()->second));
		_neighborsModified = true;
	}
}


void Node::changeSensorsRef(int oldSensorId, int activeSensorId)
{
	for(unsigned int i=0; i<_sensors.size(); ++i)
	{
		if(_sensors[i] == oldSensorId)
		{
			_sensors[i] = activeSensorId;
			_sensorsChanged.insert(std::make_pair(oldSensorId, activeSensorId));
		}
	}
}

bool Node::isEmpty() const
{
	return !_sensors.size();
}


} //namespace rtabmap
